#include <p18cxxx.h>
#include <delays.h> 	/* this is needed for the delay functions */
#include <usart.h>	/* include USART peripherial functions */
#include <sw_uart.h>	/* include software UART peripherial functions */
#include <stdlib.h>
#include <capture.h>
#include <timers.h>
#include "timedelays.h"
#include "commands.h"


// Configuration bits, OSCillator=HighSpeed, WatchDogTimer=OFF, LowVoltageProgramming=OFF
#pragma config OSC=HS, WDT=OFF, LVP=OFF


/* Defines */
#define ENCODER_A_LEFT	PORTBbits.RB6	// pin for left wheel, first IR receiver
#define ENCODER_B_LEFT	PORTDbits.RD2	// pin for left wheel, second IR receiver
#define ENCODER_A_RIGHT	PORTBbits.RB7	// pin for right wheel, first IR receiver
#define ENCODER_B_RIGHT	PORTDbits.RD3	// ping for right wheel, second IR receiver
#define MC_RESET	LATDbits.LATD1	// pin for controlling resetting of the dual motorcontroller
#define MC_ERROR	LATDbits.LATD0	// pin used to indicate an error has occured
#define PING		LATCbits.LATC2	// pin used for PING)))) sensor
#define IR_BEACON	PORTCbits.RC3	// pin used to find beacon
#define BUMP_SENSOR	PORTCbits.RC4	// pin used for bump sensor
#define IR_SEEN		LATCbits.LATC1	// pin used to tell the spot that the IR beacon is visible
#define NUM_DIVISIONS	20		// # of ticks produced per 1 revolution of a wheel
#define MC_START_BYTE   0x80		// start byte used for motorcontroller commands
#define FORWARD		0x01		// forward direction passed to motorcontroller
#define REVERSE		0x00		// reverse direction passed to motorcontroller
#define LEFT_MOTOR	0x01		// motor # for the left motor based on where they are plugged in on the MC
#define RIGHT_MOTOR	0x00		// motor # for the right motor based on where they are plugged in on the MC
#define MIN_TPS		10		// a minimum ticks per second to maintain
#define MAX_TPS		60		// the maximum ticks per second allowed (basically a governor)
#define FALSE		0x00
#define TRUE		0x01


/* The following are defines used for keeping track of what state the MC is in */
#define STATE_IDLE		0x00
#define STATE_FORWARD		0x01
#define STATE_REVERSE		0x02
#define STATE_ROTATECW		0x03
#define STATE_ROTATECCW		0x04
#define STATE_TURNLEFT		0x05
#define STATE_TURNRIGHT		0x06
#define STATE_FWD_RPS		0x07
#define STATE_REV_RPS		0x08
#define STATE_FWD_STRAIGHT	0x09

volatile int leftenc_ticks = 0;			// keeps track of # of ticks left wheel has moved
volatile int rightenc_ticks = 0;		// keeps track of # of ticks right wheel has moved
volatile int tps_right = 0;			// keeps track of the ticks per second the right wheel moves
volatile int tps_left = 0;			// keeps track of the ticks per second the left wheel moves
int lasttps_right = 0;				// aids in determining how many ticks have elapsed since last time
int lasttps_left = 0;				// aids in determining how many ticks have elapsed since last time
char tps_offset = 0;				// allows right wheel to move slower or faster than the left
unsigned char last_tick0;			// used to determine if the right wheel has moved or not
unsigned char last_tick1;			// used to determine if the left wheel has moved or not
unsigned char current_state = STATE_IDLE;	// variable to keep track of the MC's current state
volatile unsigned char left_speed = 0;		// variable to keep track of the left wheel's speed on the MC
volatile unsigned char right_speed = 0;		// variable to keep track of the right wheel's speed on the MC
volatile unsigned char left_dir = FORWARD;	// variable to keep track of the direction the left wheel is moving
volatile unsigned char right_dir = FORWARD;	// variable to keep track of the direction the right wheel is moving
volatile unsigned char control_tps = 0;		// if this is set to 1, the tps_isr will attempt to equalize the wheel speeds
volatile unsigned char serialBuffer[5]; 	// buffer for storing received data from the USART
volatile unsigned char readPos = 0;		// index that the read position is at for the circular serialBuffer
unsigned char writePos = 0;			// index that the write position is at for the circular serialBuffer
unsigned char bRampSpeed = 1;
unsigned char tickSpeed = 120;
unsigned char insane = 0;

/* Function Prototypes */
// isr functions
void encoder_isr(void);
void tps_isr(void);

// motor functions
void mc_move_motor(unsigned char motor_num, unsigned char dir, unsigned char speed);
void mc_move_forward(unsigned char speed);
void mc_move_reverse(unsigned char speed);
void mc_turn_left(unsigned char speed);
void mc_turn_right(unsigned char speed);
void mc_rotate_left(unsigned char speed);
void mc_rotate_right(unsigned char speed);
void mc_inc_left(void);
void mc_dec_left(void);
void mc_inc_right(void);
void mc_dec_right(void);
void mc_init(void);
void mc_brake(void);
void mc_reset();
void mc_turn_right_ticks(unsigned int ticks);
void mc_turn_left_ticks(unsigned int ticks);
void mc_move_forward_ticks(unsigned int ticks);
void mc_move_reverse_ticks(unsigned int ticks);
void mc_rotate_left_ticks(unsigned int ticks);
void mc_rotate_right_ticks(unsigned int ticks);
void getCurrentTicks(void);
void getCurrentTPS(void);

// serial buffer functions
void WriteBuffer(unsigned char data);
unsigned char ReadBuffer(void);
unsigned char DataRdyBuffer(void);

// pic init routine
void pic_initialize(void);
void SanityCheck();

#pragma code high_vector = 0x08
void high_ISR(void)
{
	_asm goto encoder_isr _endasm
}
#pragma code


#pragma code _LOW_INTERRUPT_VECTOR = 0x18
void low_ISR(void)
{
	_asm goto tps_isr _endasm
}
#pragma code


/* Interrupt to tally up the # ticks that occur on each encoder wheel */
/* this is a high priority interrupt and will pre-empt low priority interrupts */
#pragma interrupt encoder_isr	// high priority interrupt handler
void encoder_isr( void )
{
	char portb = PORTB;
	INTCON &= 0b11111110;	// reset RBIF interrupt
	if( (ENCODER_A_RIGHT != last_tick0) )
	{
		/* If ENCODER_A_RIGHT == ENCODER_B_RIGHT then B must have changed just before A */
		/* and therefore the encoder wheel is moving in the forward direction */
		/* otherwise the encoder wheel is moving in the reverse direction 	  */
		last_tick0 = ENCODER_A_RIGHT;
		if( ENCODER_A_RIGHT == ENCODER_B_RIGHT )
		{
			rightenc_ticks--;
		}
		else
		{
			rightenc_ticks++;
		}
	}
	if( (ENCODER_A_LEFT != last_tick1) )
	{
		/* If ENCODER_A_LEFT == ENCODER_B_LEFT then B must have changed just before A */
		/* and therefore the encoder wheel is moving in the forward direction */
		/* otherwise the encoder wheel is moving in the reverse direction 	  */
		last_tick1 = ENCODER_A_LEFT;
		if( ENCODER_A_LEFT == ENCODER_B_LEFT )
		{
			leftenc_ticks--;
		}
		else
		{
			leftenc_ticks++;
		}
	}
}


/* Interrupt to caclulate ticks per second and to control the speed of */
/* the wheels if control_tps = 1				       */
#pragma interrupt tps_isr  // low priority interrupt handler
void tps_isr(void)
{
	static char left_count[] = {0,0,0,0,0,0,0,0,0,0};
	static char right_count[] = {0,0,0,0,0,0,0,0,0,0};
	static char index = 0;
	char serData = 0;
	static char irCounter = 0;
	static char seenCounter = 0;
	static unsigned char maxtps_div10 = MAX_TPS / 10;
	static unsigned char last_left_speed = 0;
	static unsigned char last_right_speed = 0;

	if(PIR1bits.TMR1IF)
	{
		PIR1bits.TMR1IF = 0;	// reset TMR1 interrupt flag
		TMR1H = 0b00001011;		// Timer1 initialized to 3036
		TMR1L = 0b11011100;		// to cause a timer interrupt every 100ms (10 times per second)
		
		if(IR_BEACON == 0)
		{
			if(seenCounter >= 2)
			{
				IR_SEEN = 1;
				irCounter = 0;
				seenCounter = 0;
			}
			seenCounter++;
		}
		else if(irCounter >= 5)
		{
			IR_SEEN = 0;
			irCounter = 0;
		}
		irCounter++;
		
		// since we want the # of ticks per second, we need to multiply by 10 since
		// the timer goes off every 1/10th of a second.
		left_count[index] = (leftenc_ticks - lasttps_left);
		right_count[index] = (rightenc_ticks - lasttps_right);
		lasttps_left = leftenc_ticks;
		lasttps_right = rightenc_ticks;

		if(current_state == STATE_REVERSE || current_state == STATE_ROTATECCW)
			left_count[index] = left_count[index] * (-1);
		if(current_state == STATE_REVERSE || current_state == STATE_ROTATECW)
			right_count[index] = right_count[index] * (-1);

		tps_left = left_count[0]+left_count[1]+left_count[2]+left_count[3]+left_count[4]+left_count[5]+left_count[6]+left_count[7]+left_count[8]+left_count[9];
		tps_right = right_count[0]+right_count[1]+right_count[2]+right_count[3]+right_count[4]+right_count[5]+right_count[6]+right_count[7]+right_count[8]+right_count[9];
	
		if(control_tps == 100)
		{
			// if we are going forward, in reverse or rotating make sure the speeds are equal
			// direction does not matter, we'll take care of that elsewhere.
			if(current_state >= STATE_FORWARD && current_state <= STATE_ROTATECCW)
			{
//				if(left_count[index] >= (maxtps_div10<<1))
//					left_speed = left_speed >> 1;
//				if(left_count[index] > maxtps_div10)
//					left_speed--;
				
				if(right_count[index] > left_count[index])
				{
					if(right_speed > left_speed)
						right_speed = left_speed;
					if(right_speed > 0)
						right_speed--;
				}
				else if(right_count[index] < left_count[index])
				{
					if(right_speed < left_speed)
						right_speed = left_speed;
					if(right_speed < 127)
						right_speed++;
				}
			}
			
			if(current_state == STATE_TURNRIGHT)
			{
				if(left_count[index] >= (maxtps_div10<<1))
					left_speed = left_speed >> 1;
				if(left_count[index] > maxtps_div10)
					left_speed--;
			}
			
			if(current_state == STATE_TURNLEFT)
			{
				if(right_count[index] >= (maxtps_div10<<1))
					right_speed = right_speed >> 1;
				if(right_count[index] > maxtps_div10)
					right_speed--;
			}
		}
		
		index++;
		if(index >= 10)
		{
			index = 0;
//			if(bRampSpeed)
//			{
				// if we are here that means our speed is non zero, but our wheels are not turning
				// so let's ramp them up by 5.
				if(left_speed != 0 && tps_left == 0 && left_speed < 127)
				{
					left_speed += 10;
					if(left_speed > 127)
					{
						left_speed = 127;
						mc_move_motor(LEFT_MOTOR, left_dir, left_speed);
					}
				}
				if(right_speed !=0 && tps_right == 0 && right_speed < 127)
				{
					right_speed += 10;
					if(right_speed > 127)
					{
						right_speed = 127;
						mc_move_motor(RIGHT_MOTOR, right_dir, right_speed);
					}
				}
				if((left_speed >= 127 || right_speed >= 127) && tps_left == 0 && tps_right == 0)
				{
					mc_brake();
					MC_ERROR = 1;
				}
			
//			}
		}
			
		// check if the speeds have changed since last time and if so send the command
		// to the appropriate motor to change its speed.
//		if(last_left_speed != left_speed)
//		{
//			last_left_speed = left_speed;
//			mc_move_motor(LEFT_MOTOR, left_dir, left_speed);
//		}
//		if(last_right_speed != right_speed)
//		{
//			last_right_speed = right_speed;
//			mc_move_motor(RIGHT_MOTOR, right_dir, right_speed);
//		}
//		SanityCheck();
	}

	else if(PIR1bits.RCIF)
	{
		serData = RCREG;
		if(serData != 128)
			WriteBuffer(serData);
		else
			MC_ERROR = 1;
	}
}


void pic_initialize(void)
{
	/* set RD0 and RD1 as outputs */
	LATD &= 0b11111100;
	TRISD &= 0b11111100;
	MC_RESET = 1;	// pull MC_RESET high so the MC is ready to go.
	MC_ERROR = 1;

	/* set RD2 and RD3 as inputs */
	TRISD |= 0b00001100;

	TRISB |= 0b11000000;	// RB6 & RB7 are inputs


	/* Open USART @ 19,200bps with Fosc=20MHz */
	/* Remember to change the paramaters to   */
	/* match the clock speed being used.	  */
	/* last param should be 64 for 19,200bps  */
	OpenUSART( USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH, 31 );

	/* Open the software UART, also at 19,2000bps */
	OpenUART();


	RCONbits.IPEN = 1;		// enable priority levels on interrupts
	INTCON = 0b11001000;	// enable unmasked interrupts and RB change interrupt
	INTCON2 = 0b01110001; 	// disable internal pull-ups and set to high priority


	TMR1H = 0b00001011;		// Timer1 initialized to 3036
	TMR1L = 0b11011100;		// to cause a timer interrupt every 100ms
	T1CON = 0b00110001;		// Timer1 settings: 1:8 prescalar, clock = Fosc/4, Timer1 ON
	PIE1 = 0b00100001;	// enable Timer1 interrupt && USART receive interrupt
	IPR1bits.TMR1IP	= 0;	// Timer1 interrupt priority low
	IPR1bits.RCIP = 0;		// USART receive interrupt priority low

	TRISC &= 0b11111001;
	LATC &= 0b11111001;
	delay_500ms();
	last_tick0 = ENCODER_A_RIGHT; // set last_tick to whatever ENCODER0 is at this point
	last_tick1 = ENCODER_A_LEFT; // set last_tick to whatever ENCODER1 is at this point
	leftenc_ticks = rightenc_ticks = 0;
	lasttps_right = lasttps_left = 0;
	MC_ERROR = 0;

	mc_brake();
	
}


/* DelayTXBitUART for sw_uart fucntions */
void DelayTXBitUART(void)
{
	/* delay for ((((2*Fosc)/(4*baud))+1)/2) - 12 cycles */
	/* dleay for ((((2*20,000,000)/(4*9600))+1)/2)-12 ~= 509 cycles */
	
	/* delay for ((((2*Fosc)/(4*baud))+1)/2) - 12 cycles */
	/* dleay for ((((2*20,000,000)/(4*19200))+1)/2)-12 ~= 249 cycles */
	Delay100TCYx(5);
	Nop();
	Nop();
	Nop();
	Nop();
	Nop();
	Nop();
	Nop();
	Nop();
	Nop();
}


// This function has been left empty intentionally.  We are not receiving serial data through
// the sofware UART, but the library needs this function defined in order to compile.
void DelayRXHalfBitUART(void)
{
}



// This function has been left empty intentionally.  We are not receiving serial data through
// the sofware UART, but the library needs this function defined in order to compile.
void DelayRXBitUART(void)
{
}


void mc_move_motor(unsigned char motor_num, unsigned char dir, unsigned char speed)
{
    char num[4];

    if(speed > 127 || dir > 1 || motor_num > 1)
	    return;
 
    putcUART(MC_START_BYTE);
    putcUART(0x00);
    putcUART((char)(dir+(motor_num<<1)));

    putcUART(speed);
}


void mc_move_forward(unsigned char speed)
{
    mc_move_motor(LEFT_MOTOR, FORWARD, speed);
    mc_move_motor(RIGHT_MOTOR, FORWARD, speed);

    if(current_state != STATE_FWD_STRAIGHT)
	    current_state = STATE_FORWARD;
    left_dir = right_dir = FORWARD;
    left_speed = right_speed = speed;
}


void mc_move_reverse(unsigned char speed)
{
    mc_move_motor(LEFT_MOTOR, REVERSE, speed);
    mc_move_motor(RIGHT_MOTOR, REVERSE, speed);

    if(current_state != STATE_FWD_STRAIGHT)
	    current_state = STATE_REVERSE;
    left_dir = right_dir = REVERSE;
    left_speed = right_speed = speed;
}


void mc_rotate_left(unsigned char speed)
{
    mc_move_motor(LEFT_MOTOR, REVERSE, speed);
    mc_move_motor(RIGHT_MOTOR, FORWARD, speed);

    if(current_state != STATE_FWD_STRAIGHT)
	    current_state = STATE_ROTATECCW;
    left_dir = REVERSE;
    right_dir = FORWARD;
    left_speed = right_speed = speed;
}


void mc_rotate_right(unsigned char speed)
{
    mc_move_motor(LEFT_MOTOR, FORWARD, speed);
    mc_move_motor(RIGHT_MOTOR, REVERSE, speed);

    if(current_state != STATE_FWD_STRAIGHT)
	    current_state = STATE_ROTATECW;
    left_dir = FORWARD;
    right_dir = REVERSE;
    left_speed = right_speed = speed;
}


void mc_turn_left(unsigned char speed)
{
    mc_move_motor(LEFT_MOTOR, FORWARD, 0);
    mc_move_motor(RIGHT_MOTOR, FORWARD, speed);

    if(current_state != STATE_FWD_STRAIGHT)
	    current_state = STATE_TURNLEFT;
    left_dir = FORWARD;
    right_dir = FORWARD;
    left_speed = 0;
    right_speed = speed;
}


void mc_turn_right(unsigned char speed)
{
    mc_move_motor(LEFT_MOTOR, FORWARD, speed);
    mc_move_motor(RIGHT_MOTOR, REVERSE, 0);

    if(current_state != STATE_FWD_STRAIGHT)
	    current_state = STATE_TURNRIGHT;
    left_dir = FORWARD;
    right_dir = FORWARD;
    left_speed = speed;
    right_speed = 0;
}


void mc_inc_left(void)
{
	if(left_speed < 127)
		left_speed++;

	mc_move_motor(LEFT_MOTOR, left_dir, left_speed);
}


void mc_dec_left(void)
{
	if(left_speed > 0)
		left_speed--;

	mc_move_motor(LEFT_MOTOR, left_dir, left_speed);
}


void mc_inc_right(void)
{
	if(right_speed < 127)
		right_speed++;
	mc_move_motor(RIGHT_MOTOR, right_dir, right_speed);
}


void mc_dec_right(void)
{
	if(right_speed > 0)
		right_speed--;

	mc_move_motor(RIGHT_MOTOR, right_dir, right_speed);
}


void mc_brake(void)
{
	mc_move_motor(LEFT_MOTOR, !left_dir, 0);
	mc_move_motor(RIGHT_MOTOR, !right_dir, 0);
	
//	current_state = STATE_IDLE;
	left_speed = 0;
	right_speed = 0;
	left_dir = FORWARD;
	right_dir = FORWARD;
}	


void mc_init(void)
{
	putcUART(MC_START_BYTE);
	putcUART(0x02);		// change configuration
	putcUART(0x00);		// two motor mode, with motor #s 0 and 1

	MC_RESET = 0;		// cause the motor controller to reset
	delay_100ms();		// delay for 100ms to allow MC to reset
	MC_RESET = 1;		// set MC_RESET back to 1 for MC to function
}


void mc_reset(void)
{
	MC_RESET = 0;
	delay_10ms();
	MC_RESET = 1;
}


void mc_move_forward_ticks(unsigned int ticks)
{
	static int left_ticks;
	static int right_ticks;

	left_ticks = leftenc_ticks + ticks;
	right_ticks = rightenc_ticks + ticks;
	
	left_speed = tickSpeed;
	right_speed =  tickSpeed;
	
	mc_move_forward(left_speed);
	
	while(rightenc_ticks < right_ticks && leftenc_ticks < left_ticks)
	{
		if(BUMP_SENSOR)
		{
			return;
		}
		
		if(MC_ERROR || insane)
			return;
		if(leftenc_ticks >= left_ticks)
		{
			control_tps = 0;
			left_speed = 0;
			mc_move_motor(LEFT_MOTOR, FORWARD, 0);
		}
		if(rightenc_ticks >= right_ticks)
		{
			right_speed = 0;
			mc_move_motor(RIGHT_MOTOR, FORWARD, 0);
		}
	}
	
	control_tps = 0;	
	mc_brake();
}


void mc_move_reverse_ticks(unsigned int ticks)
{
	int left_ticks;
	int right_ticks;

	left_ticks = leftenc_ticks - ticks;
	right_ticks = rightenc_ticks - ticks;
	left_speed = tickSpeed;
	right_speed =  tickSpeed;
	
	mc_move_reverse(left_speed);
	
	while(rightenc_ticks > right_ticks && leftenc_ticks > left_ticks)
	
	{
		if(MC_ERROR || insane)
			return;
		if(leftenc_ticks <= left_ticks)
		{
			control_tps = 0;
			left_speed = 0;
			mc_move_motor(LEFT_MOTOR, FORWARD, 0);
		}
		if(rightenc_ticks <= right_ticks)
		{
			control_tps = 0;
			right_speed = 0;
			mc_move_motor(RIGHT_MOTOR, FORWARD, 0);
		}
	}
	
	control_tps = 0;
	mc_brake();
}


void mc_turn_left_ticks(unsigned int ticks)
{
	int target_ticks;
	 
	target_ticks = rightenc_ticks + ticks;
	
	right_speed = tickSpeed >> 1;
	right_dir = FORWARD;
	mc_turn_left(right_speed);
	

	while(rightenc_ticks < target_ticks)
	{
		if(BUMP_SENSOR)
		{
			return;
		}
		
		if(MC_ERROR || insane)
			return;
	}
	
	control_tps = 0;	
	mc_brake();
}

	
void mc_turn_right_ticks(unsigned int ticks)
{
	int target_ticks;
	 
	target_ticks = leftenc_ticks + ticks;
	
	left_speed = tickSpeed >> 1;
	left_dir = FORWARD;
	mc_turn_right(left_speed);
		
	while(leftenc_ticks < target_ticks)
	{
		if(BUMP_SENSOR)
		{
			mc_brake();
			return;
		}
		
		if(MC_ERROR || insane)
			return;
	}
	
	control_tps = 0;	
	mc_brake();
}
	

void mc_turn_right_ticks_reverse(unsigned int ticks)
{
	int target_ticks;
	 
	target_ticks = rightenc_ticks - ticks;
	
	right_speed = tickSpeed >> 1;
	right_dir = REVERSE;
	mc_move_motor(RIGHT_MOTOR, right_dir, right_speed);
	
	while(rightenc_ticks > target_ticks)
	{
		if(MC_ERROR || insane)
			return;
	}
	
	control_tps = 0;	
	mc_brake();
}

	
void mc_turn_left_ticks_reverse(unsigned int ticks)
{
	int target_ticks;
	 
	target_ticks = leftenc_ticks - ticks;
	
	left_speed = tickSpeed >> 1;
	left_dir = REVERSE;
	mc_move_motor(LEFT_MOTOR, left_dir, left_speed);
	
	while(leftenc_ticks > target_ticks)
	{
		if(MC_ERROR || insane)
			return;
	}
	
	control_tps = 0;	
	mc_brake();
}

	
void mc_rotate_left_ticks(unsigned int ticks)
{
	int left_ticks;
	int right_ticks;
	int i;

	left_ticks = leftenc_ticks - ticks;
	right_ticks = rightenc_ticks + ticks;
	left_speed = tickSpeed;
	right_speed = tickSpeed;
	
	mc_rotate_left(left_speed);
	
	while(rightenc_ticks < right_ticks || leftenc_ticks > left_ticks)
	{
		if( ((leftenc_ticks > left_ticks && leftenc_ticks <= left_ticks+5) ||
		    (rightenc_ticks < right_ticks && rightenc_ticks >= right_ticks-5)) && ticks > 1 )
		{
		
			if(tickSpeed <= 60)
				mc_rotate_left(tickSpeed-20);
			else
				mc_rotate_left(tickSpeed>>1);
		}
		
		if(MC_ERROR || insane)
			return;
		
		if(BUMP_SENSOR)
		{
			mc_brake();
			mc_move_reverse_ticks(5);
			return;
		}
		if(leftenc_ticks <= left_ticks)
		{
			control_tps = 0;
			left_speed = 0;
			mc_move_motor(LEFT_MOTOR, FORWARD, 0);
		}
		if(rightenc_ticks >= right_ticks)
		{
			control_tps = 0;
			right_speed = 0;
			mc_move_motor(RIGHT_MOTOR, FORWARD, 0);
		}
	}
/*
	for(i = 0; i < ticks; i++)
	{
		mc_turn_left_ticks(1);
		mc_turn_left_ticks_reverse(1);
	}	
	control_tps = 0;	
*/
	if(ticks > 2)
		mc_rotate_right(tickSpeed);
	mc_brake();
}


void mc_rotate_right_ticks(unsigned int ticks)
{
	int left_ticks = 0;
	int right_ticks = 0;
	int i;

	left_ticks = leftenc_ticks + ticks;
	right_ticks = rightenc_ticks - ticks;
	left_speed = tickSpeed;
	right_speed = tickSpeed;
	
	mc_rotate_right(left_speed);
	
	while(rightenc_ticks > right_ticks || leftenc_ticks < left_ticks)
	{
		if( (leftenc_ticks < left_ticks && leftenc_ticks >= left_ticks-5) ||
		    (rightenc_ticks > right_ticks && rightenc_ticks <= right_ticks+5) && ticks > 2)
		{
		
//			if(tickSpeed > 25)
//				mc_rotate_right(25);
			if(tickSpeed <= 60)
				mc_rotate_right(tickSpeed-20);
			else
				mc_rotate_right(tickSpeed>>1);
		}
		if(MC_ERROR || insane)
			return;
		if(BUMP_SENSOR)
		{
			mc_brake();
			mc_move_reverse_ticks(5);
			return;
		}
		if(leftenc_ticks >= left_ticks)
		{
			control_tps = 0;
			left_speed = 0;
			mc_move_motor(LEFT_MOTOR, FORWARD, 0);
		}
		if(rightenc_ticks <= right_ticks)
		{
			control_tps = 0;
			right_speed = 0;
			mc_move_motor(RIGHT_MOTOR, FORWARD, 0);
		}
	}

/*	for(i = 0; i < ticks; i++)
	{
		mc_turn_right_ticks(1);
		mc_turn_right_ticks_reverse(1);
	}	
*/	
	control_tps = 0;	
	if(ticks > 2)
		mc_rotate_left(tickSpeed);
	mc_brake();
}


// This function will output the current # of ticks per wheel via the serial connection.
// The output is ascii text with the left value first then the right, seperated by a comma
void getCurrentTicks(void)
{
	int left = leftenc_ticks;
	int right = rightenc_ticks;
	unsigned char str[8];
	
	itoa(left, (char*)str);
	putsUSART((char*)str);
	putcUSART(',');
	itoa(right, (char*)str);
	putsUSART((char*)str);
}


// This function will output the current tps per wheel via the serial connection.
// The output is ascii text with the left value first then the right, seperated by a comma
void getCurrentTPS(void)
{
	char left = tps_left;
	char right = tps_right;
	unsigned char str[5];
	
	btoa(left, (char*)str);
	putsUSART((char*)str);
	putcUSART(',');
	btoa(right, (char*)str);
	putsUSART((char*)str);
}


// This function writes a byte to the serialBuffer and increments the writePos to the next spot
// we increment writePos and then and it with 32 to cause it to go back to zero if writePos
// becomes 32.  We could just use modulo (%) but this is faster since no arithmetic is performed
void WriteBuffer(unsigned char data)
{
	serialBuffer[writePos] = data;
	writePos = (writePos + 1) % 5;
	if(writePos == readPos)
		readPos = (readPos + 1) % 5;
}


// This function reads a byte from the serial buffer and increments the readPos if it is not currently
// equal to the writePos, which means no data is available to consume.
unsigned char ReadBuffer(void)
{
	unsigned char data = serialBuffer[readPos];
	if(readPos != writePos)
		readPos = (readPos + 1) % 5;
		
	return data;
}


// This function returns true if there is data in the serial buffer.  This is accomplished by
// returning whether readPos != writePos, if it is, we return false(0), otherwise true(1).
unsigned char DataRdyBuffer(void)
{
	return (readPos != writePos);
}


void SanityCheck()
{
/*
	switch(current_state)
	{
		case STATE_IDLE:
			if(tps_right || tps_left)
				insane = 1;
			break;
		case STATE_FORWARD:
			if(left_dir != FORWARD || right_dir != FORWARD)
				insane = 1;
			break;
		case STATE_REVERSE:
			if(left_dir != REVERSE || right_dir != REVERSE)
				insane = 1;
			break;
		case STATE_ROTATECW:
			if(left_dir != FORWARD || right_dir != REVERSE)
				insane = 1;
			break;
		case STATE_ROTATECCW:
			if(left_dir != REVERSE || right_dir != FORWARD)
				insane = 1;
			break;
		case STATE_TURNLEFT:
			if(tps_left || right_dir != FORWARD)
				insane = 1;
			break;
		case STATE_TURNRIGHT:
			if(tps_right || left_dir != FORWARD)
				insane = 1;
			break;
		case STATE_FWD_RPS:
			if(left_dir != FORWARD || right_dir != FORWARD)
				insane = 1;
		case STATE_REV_RPS:
			if(left_dir != REVERSE || right_dir != REVERSE)
				insane = 1;
		case STATE_FWD_STRAIGHT:
			if(left_dir != FORWARD || right_dir != FORWARD)
				insane = 1;
		default:
			break;
	}
*/	
	if((tps_right > 20 && tps_left < 10) || (tps_left > 20 && tps_right < 10))
		insane = TRUE;
		
	if(insane)
		mc_brake();
}


void GoStraightTicks(char ticks)
{
	int curr_diff, tick_diff;
	int left_tick_count, right_tick_count;
	unsigned char done = FALSE;
	
	tick_diff = leftenc_ticks - rightenc_ticks;
	left_tick_count = leftenc_ticks + ticks;
	right_tick_count = rightenc_ticks + ticks;
	
	
	while(done != TRUE)
	{	
		curr_diff = leftenc_ticks - rightenc_ticks;
		
		if(curr_diff == tick_diff && (leftenc_ticks < left_tick_count && rightenc_ticks < right_tick_count))
			mc_move_forward_ticks(2);
		else if(curr_diff > tick_diff && (rightenc_ticks < right_tick_count))
			mc_turn_left_ticks(curr_diff - tick_diff);
		else if(curr_diff < tick_diff && (leftenc_ticks < left_tick_count))
			mc_turn_right_ticks(tick_diff - curr_diff);
		else
			done = TRUE;
		
		current_state = STATE_FWD_STRAIGHT;
		
		if(BUMP_SENSOR)
		{
			mc_brake();
			done = TRUE;
			mc_move_reverse_ticks(5);
		}
		
		if(MC_ERROR || insane)
			done = TRUE;
	}

	mc_brake();
}	

void main(void)
{
	char value[6];
	char cmd[4];
	char cmd_idx = 0;
	int left_tick_count = 0;
	int right_tick_count = 0;
	int tick_diff = 0;
	int curr_diff = 0;

	pic_initialize();

//	SerialTest();	
//	mc_test_run();
//	lookingForErrors();
//	testSonar();
	
//	WriteBuffer('!');
//	WriteBuffer(_MC_MOVE_FWD_STRAIGHT);
//	WriteBuffer(68);
	
	while(1)
	{
		SanityCheck();
		if(current_state == STATE_IDLE && (tps_left || tps_right))
			mc_brake();

		if(MC_ERROR || insane)
		{
			mc_reset();
			delay_1s();
			mc_reset();
			delay_1s();
			mc_reset();
			mc_brake();
			MC_ERROR = 0;
		}
		
		// check if the receive buffer has overflowed and if so reset the USART for continuous receive again.
		if ( RCSTAbits.OERR == 1) 
		{ 
			RCSTAbits.CREN = 0;  //clearing CREN clears any Overrun (OERR) errors 
			RCSTAbits.CREN = 1;  //re-enable continuous USART receive 
		}
		
		if(DataRdyBuffer())
		{
			cmd[cmd_idx] = ReadBuffer();
			
			if(cmd[cmd_idx] == '!' && cmd_idx != 0)
			{
				cmd[0] = '!';
				cmd_idx = 0;
			}
			
			cmd_idx++;
		}
		
		if(cmd_idx == 3)
		{
			cmd_idx = 0;
			if(cmd[0] == '!')	// start byte
			{
				while(BusyUSART());
				putcUSART(cmd[0]+1);
				while(BusyUSART());
				putcUSART(cmd[1]);
				while(BusyUSART());
				putcUSART(cmd[2]);
				switch(cmd[1])
				{
					case _MC_MOVE_FORWARD:
						control_tps = 1;
						mc_move_forward(cmd[2]);
						break;
					
					case _MC_MOVE_REVERSE:
//						control_tps = 1;
//						mc_move_reverse(cmd[2]);
//						control_tps = 1;
						break;
					
					case _MC_TURN_LEFT:
//						mc_turn_left(cmd[2]);
						break;
					
					case _MC_TURN_RIGHT:
//						mc_turn_right(cmd[2]);
						break;
					
					case _MC_ROTATE_CCW:
						control_tps = 1;
						mc_rotate_left(cmd[2]);
						break;
					
					case _MC_ROTATE_CW:
						control_tps = 1;
						mc_rotate_right(cmd[2]);
						break;
					
					case _MC_INC_LEFT_SPEED:
//						control_tps = 0;
//						current_state = STATE_FORWARD;
//						mc_inc_left();
						break;
					
					case _MC_DEC_LEFT_SPEED:
//						control_tps = 0;
//						current_state = STATE_FORWARD;
//						mc_dec_left();
						break;
					
					case _MC_INC_RIGHT_SPEED:
//						control_tps = 0;
//						current_state = STATE_FORWARD;
//						mc_inc_right();
						break;
					
					case _MC_DEC_RIGHT_SPEED:
//						control_tps = 0;
//						current_state = STATE_FORWARD;
//						mc_dec_right();
						break;
					
					case _MC_MOVE_FORWARD_TICKS:
						control_tps = 1;
						mc_move_forward_ticks((unsigned int)cmd[2]);
						break;
					
					case _MC_MOVE_REVERSE_TICKS:
						control_tps = 1;
						mc_move_reverse_ticks((unsigned int)cmd[2]);
						break;
					
					case _MC_BRAKE:
						mc_brake();
						current_state = STATE_IDLE;
						control_tps = 0;
						break;
					
					case _MC_TURN_LEFT_TICKS:
//						mc_turn_left_ticks((unsigned int)cmd[2]);
						break;
					
					case _MC_TURN_RIGHT_TICKS:	
//						mc_turn_right_ticks((unsigned int)cmd[2]);
						break;
					
					case _MC_ROTATE_CCW_TICKS:
						mc_rotate_left_ticks((unsigned int)cmd[2]);
						break;
					
					case _MC_ROTATE_CW_TICKS:
						mc_rotate_right_ticks((unsigned int)cmd[2]);
						break;
					
					case _MC_GET_NUM_TICKS:
						getCurrentTicks();
						break;
					
					case _MC_GET_TPS:
						getCurrentTPS();
						break;
						
					case _MC_TURN_LEFT_REVERSE:
//						current_state = STATE_TURNRIGHT;
//						mc_turn_left_ticks_reverse((unsigned int)cmd[2]);
						break;
					
					case _MC_TURN_RIGHT_REVERSE:
//						current_state = STATE_TURNLEFT;
//						mc_turn_right_ticks_reverse((unsigned int)cmd[2]);
						break;
						
					case _MC_MOVE_FWD_STRAIGHT:
						GoStraightTicks(cmd[2]);
						break;
						
					case _MC_INC_DIFF:
//						if(current_state == STATE_FWD_STRAIGHT)
//						{
//							if(cmd[2] > 0)
//								tick_diff += cmd[2];
//							else
//								tick_diff++;
//						}
//						else
//						{
//							mc_brake();
//							left_tick_count = leftenc_ticks;
//							right_tick_count = rightenc_ticks;
//							tick_diff = left_tick_count - right_tick_count;
//							current_state = STATE_FWD_STRAIGHT;
//						}
						break;
					
					case _MC_DEC_DIFF:
//						if(current_state == STATE_FWD_STRAIGHT)
//						{
//							if(cmd[2] > 0)
//								tick_diff -= cmd[2];
//							else
//								tick_diff--;
//						}
//						else
//						{
//							mc_brake();
//							left_tick_count = leftenc_ticks;
//							right_tick_count = rightenc_ticks;
//							tick_diff = left_tick_count - right_tick_count;
//							current_state = STATE_FWD_STRAIGHT;
//						}
						break;
					
					case _MC_SET_MAX_SPEED:
						if(cmd[2] > 10 && cmd[2] <= 127)
							tickSpeed = cmd[2];
						break;
							
					
					default:	
					break;
				}
			}
			// send the command back through the serial connection to signal it was completed
			cmd[3] = 0;
//			putsUSART(cmd);
			while(BusyUSART());
			putcUSART(cmd[0]+2);
			while(BusyUSART());
			putcUSART(cmd[1]);
			while(BusyUSART());
			putcUSART(cmd[2]);
			cmd[0] = cmd[1] = cmd[2] = 0;
		}
	}
}
